#include <Servo.h>
#include <Wire.h>

//adresses
int HMC6352Address = 0x42;
int slaveAddress;

//constantes de réglage
int errcapadm=10;
int neutreservo=90;
int courseservo=90;
int nbrectrlcap=10;
int tempslissage=100;
int nbrepointslissage=20;

//Variables globales
int posservo=90;

//Objets
Servo servo;  // create servo object to control a servo
 
void setup()
{
  servo.attach(2);  // attaches the servo on pin 2 to the servo object
  //alimentation boussole:
  pinMode(16, OUTPUT);
  digitalWrite(16, LOW);
  pinMode(17, OUTPUT);
  digitalWrite(17, HIGH);
  slaveAddress = HMC6352Address >> 1;   // This results in 0x21 as the address to pass to TWI
  Wire.begin();
  
  Serial.begin(9600);
}


int Boussole()
{
  byte headingData[2];
  int i, headingValue;
  Wire.beginTransmission(slaveAddress);
  Wire.send("A");              // The "Get Data" command
  Wire.endTransmission();
  delay(10);                   // The HMC6352 needs at least a 70us (microsecond) delay
  // after this command.  Using 10ms just makes it safe
  // Read the 2 heading bytes, MSB first
  // The resulting 16bit word is the compass heading in 10th's of a degree
  // For example: a heading of 1345 would be 134.5 degrees
  Wire.requestFrom(slaveAddress, 2);        // Request the 2 byte heading (MSB comes first)
  i = 0;
  while(Wire.available() && i < 2)
  { 
    headingData[i] = Wire.receive();
    i++;
  }
  headingValue = headingData[0]*256 + headingData[1];  // Put the MSB and LSB together
  headingValue = headingValue/ 10; //cap sur 360°
  return headingValue;
} 


int BoussoleLissee(){
  
  int somme=0;
  int t[nbrepointslissage];
  
  for (int i=0; i<nbrepointslissage; i++){//boucle pendant laquelle on prélève un cap tous les tempslissage; on prélève nbrepointslissage valeurs.
    t[i]=Boussole();
    delay(tempslissage);
    //Serial.println(t[i]);
   }
  for (int i=0; i<nbrepointslissage; i++){//somme de tous les caps prélevés ci-dessu
    somme=somme+t[i];
   } 
  double valeurmoyenne = somme/nbrepointslissage;//moyenne
  int v = (int) valeurmoyenne;//transformation en entier
  //Serial.print("valmoy : ");
  //Serial.println(v);
  return v;
 }


boolean SuiviCap(int cap){
int errcap;
int correrr=10;
int capboussole;

for(int i=0; i<nbrectrlcap; i++){    //boucle de correction du cap
//calcul de l'erreur de cap
capboussole= BoussoleLissee();
  if(capboussole<cap){
    if(cap-capboussole<((360+capboussole)-cap)){
      errcap=cap-capboussole;
    }
    else{
      errcap=(360+capboussole)-cap;
    }
  }
  else{
    if(capboussole-cap<((360+cap)-capboussole)){
      errcap=capboussole-cap;
    }
    else{
      errcap=(360+cap)-capboussole;
    }
  }
  //Si le cap est correct
  if(errcap<errcapadm){
  //    Serial.println("Cap correct");
  return true;
  }
  else{
  // Calcul de l'angle de correction:
  correrr=errcap/10;
  //incrémenter ou décrémenter l'angle du safran
  if(cap>180){
     if (capboussole>(cap-180) && capboussole<cap){
       //virer sur babord
       if (posservo+correrr<neutreservo+courseservo){
         posservo=posservo+correrr;
       }
       else{
        posservo=neutreservo+courseservo; 
       }
       servo.write(posservo);
   }
     else{
       //virer sur tribord
       if (posservo-correrr>neutreservo-courseservo){
         posservo=posservo-correrr;
       }
       else{
        posservo=neutreservo-courseservo; 
       }
       servo.write(posservo);
   }
  }
  else{
     if (capboussole<cap || capboussole>cap+180){
       //virer sur babord
       if (posservo+correrr<neutreservo+courseservo){
         posservo=posservo+correrr;
       }
       else{
       posservo=neutreservo+courseservo; 
       }
       servo.write(posservo);
   }
     else{
       //virer sur tribord
       if (posservo-correrr>neutreservo-courseservo){
         posservo=posservo-correrr;
       }
       else{
        posservo=neutreservo-courseservo; 
       }
       servo.write(posservo);
   }
  }
  }
//  Serial.print("Capboussole ");
//  Serial.println(capboussole);
//  Serial.print("Posservo ");
//  Serial.println(posservo);
//  Serial.print("Erreur ");
//  Serial.println(errcap);
}
return false;
}

void loop(){
  boolean test;
  test=SuiviCap(50);
  if(test==true){
    Serial.println("Cap traite");
  }
  else{
    Serial.println("Impossible de corriger le cap");
  }
}
